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(54) Title: METHOD OF TRACKING AND SENSING POSITION OF OBJECTS 
(57) Abstract 

A spatial position and 
orientation sensor based on a 
three-dimensional imaging system 
provides information describing 
the actual three-dimensional spatial 
position relative to the sensor of 
objects in the field of view of the 
sensor. The sensor thus directly 
determines its location relative to 
its surroundings. Movement of the 
sensor, and thus movement of an 
object carrying the sensor, relative 
to the surroundings is detected by 
comparing repeated determinations 
of the location in three dimensions 
of fixed features in the surroundings 
relative to the sensor. An algorithm 
is used to calculate changes in die 
position of features in the field of 
view of the sensor relative to the 
sensor. Knowledge of changes in 
position relative to fixed features is 
used to determine movement in three 
dimensions of the sensor or an object 
carrying the sensor relative to the 
fixed features in the surroundings. The features in the surroundings may be natural or artificial. The true extent of the movement of the 
sensor in three dimensions is directly determined from the change in position and orientation of the sensor relative to the fixed features in 
the surroundings. The extent of the movement is used to determine the new position and orientation of the sensor relative to a previous 
position. When starting from a predetermined location the actual position and orientation of the sensor may be reliably tracked over a 
period of time by accumulating the measured movements over that time. 
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METHOD OF TRACKING AND SENSING POSITION OF OBJECTS 
Technical Field 

The present invention relates to sensing and tracking the position of 
objects in an environment, particularly but not exclusively an environment where 
5 the features of the environment are not well controlled or predefined. 
Background Art 

In order to undertake tasks in an environment that is known or unknown a 
priori, systems controlling machinery which must move in or through the 
environment, for example mining equipment, must have knowledge of the 

1 0 absolute or true position and orientation of the machinery in relation to the 
surroundings of the machinery. For some instances of controlled operation 
whereby elements of the control are exercised by a human operator and for 
autonomous operation, a control system must be equipped with a means to 
determine the absolute or true position and orientation of the machinery and the 

15 complete relationship of the machinery to the surroundings it is operating in. 

The environment may be composed of natural or artificial features and 
may be complex thus possessing little or no regular structure. In addition, some 
features in the surroundings may be in motion relative to other features nearby. 
Many methods have been disclosed, which enable a machine to navigate 

20 (determine its position and control its position) by using artificial features in a 
predetermined environment. For example, US patent 4831539 to Hagenbuch 
discloses a system for identifying the location of a vehicle using distinctive 
signposts located at predefined positions. US patent Nos 4811228 to Hyyppa 
and 5367458 to Roberts utilise predefined target or signpost devices which are 

25 identified in some way by the vehicle to provide location information. 

US patent No. 5,051,906 to Evans, Jr., et al discloses an apparatus and 
method which provides for the determination of a vehicle's orientation and 
position in an environment, such as a hallway, from an image of a retroreflective 
ceiling feature. 

30 These systems are inherently limited in their application by the 

requirement to determine the environment prior to operation of the machine and 
in many cases install artificial features which are used to determine the position 
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of the machine within the defined environment. For example, it may be possible 
in a warehouse environment to provide a well controlled environment without 
unexpected features - however, this is much more difficult in a changing 
environment such as an open cut mine or underground mine, where the shape 
5 and location of objects are inherently in a state of change. 

Other methods which enable a machine to navigate using inertial 
navigation systems have been disclosed. The operation of inertial navigation 
systems is usually based on assumptions about the reference frame - for 
example the rotation and revolution of the earth. Double integration of the 

1 0 acceleration determined by the navigation system often results in unacceptable 
drift in the calculated position determined by the inertial navigation system. 
Also, repeated changes in acceleration and repeated movement about a point 
tend to produce cumulative errors in inertial systems, as the next position 
assessment is based upon the previously determined value. 

15 US patent No. 4.626,995 to Lofgren et al discloses an arrangement in 

which a computer determines the Cartesian coordinates of a single light source 
through a camera attached to a vehicle. This arrangement requires that the 
height of the light source and the height of the sensor be predetermined. US 
patent No. 4,858,132 to Holmquist discloses a system in which a computer 

20 determines the bearing of a composite light source through a camera attached 
to a vehicle, and from the apparent spacing between the elements of the lights 
determines bearing and range. 

US patent No. 5,483,455 discloses an arrangement in which targets 
located at predefined positions with respect to a base reference frame are 

25 detected and the position of a vehicle relative to the target determined. Location 
of the vehicle relative to the base reference frame is determined from the 
position of the vehicle relative to the known target. 

It has been disclosed in the prior art that laser scanners may be used to 
determine position on a known path relative to a defined set of fixed reference 

30 points. In one embodiment, a laser or light transmitter scans a volume in which 
are located characteristic features consisting of reflectors intended to direct the 
emitted light back to a sensor located with the transmitter. The prior art also 
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teaches the use of laser scanners to determine position relative to natural 
features and to memorise the position of such features in a two dimensional 
plane only in order to navigate between them. 

It is also disclosed in the prior art to use scanning laser rangefinders to 
5 position equipment 2-dimensionally in a constrained environment such as a 
tunnel. Such techniques, known as wall following, are used for obstacle 
detection and the avoidance of collisions with fixed features such as walls and 
other obstacles. Such techniques may also be used to fuse data referenced to a 
known local environment with data from a dead reckoning system such as an 
10 inertial navigation system (INS) by periodically resetting the INS see 
"Experiments In Autonomous Underground Guidance", Scheding S., Nebot E., 
Stevens M.,Durrant-Whyte H. f Roberts J., Corke P., Cunningham J., Cook B; in 
IEEE Conference on Robotics and Automation, Albuquerque 1997. 

In "An Experiment in Guidance and Navigation of an Autonomous Robot 
15 Vehicle", Blanche IEEE Transactions on Robotics and Automation, Vol 7, No 2, 
April 1991, an experimental vehicle designed to operate autonomously within a 
structured office or factory environment is discussed. The disclosed device uses 
an odometry and steering angle based primary system, with a laser rangefinder 
used to correct this with respect to a predetermined 2-D map of the environment, 
20 and remotely generated path plans. 

Methods have been disclosed also, which enable a machine to avoid 
collision with features in its environment, obstacle avoidance, that is, to 
determine its position and control its position relative to those features in such a 
manner as to avoid contact with the features. For example, US patent 5,758,298 
25 to Guldner discloses an autonomous navigation system for a mobile robot or 
manipulator. In the description of this patent all operations are performed on the 
local navigation level in the robot coordinate system. US patent No. 4,954,962 
to Evans, Jr., et al discloses a navigation control system of a robot which inputs 
data from a vision system and infers therefrom data relating to the configuration 
30 of the environment which lies in front of the robot so that the robot may navigate 
to a desired point without collision with obstacles or features in its environment. 
Obstacle avoidance or collision avoidance systems are not required to 
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determine and track the true position and orientation of the machine within the 
defined environment and therefore cannot be used for navigation and guidance 
except in the very local sense of avoiding collisions. 

A plurality of methods have been disclosed involving the use of methods 
5 such as predetermined or installed reference points, stored maps of the local 
environment, infrastructure such as the Global Positioning System (GPS) or 
local radio navigation systems and systems such as inertial navigation systems. 
All of these methods use infrastructure which may be integral with the immediate 
environment or external to the immediate environment and must exist or be 
10 installed. 

It is an object of the present invention to provide a location and navigation 
system which will enable a machine to operate over an extended area knowing 
its true location in that area which does not require the use of a predetermined 
reference frame or network of reference features, and is not reliant upon INS or 
1 5 GPS or similar sensing arrangements. 
Summary of Invention 

According to a first aspect, the present invention provides a method for 
determining the position of a movable object, including the steps of: 

(a) initiating the process of determining the absolute or true position of 
20 said object in 3 dimensions when said object is at a known position in 3 

dimensions ; 

(b) obtaining data indicative of the 3 dimensional location of one or more 
fixed features which may then be used as reference locations relative to said 
object via a sensing means, that is determining from said data the position in 

25 three dimensions of said one or more fixed features with respect to said object; 

(c) moving said object; 

(d) at the new position, obtaining data indicative of the new location in 3 
dimensions of said one or more fixed features relative to said object via said 
sensing means, and determining from said data the displacement of said object 

30 with respect to said one or more fixed features; 

(e) determining from said displacement of the object and said knowledge 
of the initial (or previous) position of the object the new position of the object in 
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three dimensions; 

(f) obtaining additional data indicative of the 3 dimensional location 
relative to said object of said fixed features or obtaining data indicative of 
extensions of said fixed features not previously visible to the sensors and 
5 (g) When possible or necessary i.e. if the object is moving out of view of 

previously known fixed reference features or when new fixed references are in 
view, obtaining data indicative of the 3 dimensional location of one or more new 
fixed features which may be used as new reference locations (which are 
different to the aforementioned ones) relative to said object via said sensing 
1 0 means, that is determining from said data the position of said one or more new 
fixed reference locations with respect to said object; and 

(h) repeating steps (c) to (g) as required until predefined conditions 
regarding the location of the object are fulfilled. 

Conveniently, a 3-D imaging sensor, which provides measurements of 
1 5 the true relative position in 3-D space of objects relative to the sensor and may 
additionally provide measurements of the optical reflectance of objects in the 
field of view (such as is acquired in a camera image or seen with the eye) such 
optical reflectance being registered with the spatial information collected by the 
sensor, is mounted on the movable object with a predetermined (known) 
20 orientation and position thereon. The reference locations may form part of a 
natural feature such as a definite surface and/or define such feature, as well as 
artificial bodies with features defining a 3-D object. 

According to another aspect, the present invention provides an 
autonomous vehicle, said vehicle including: 
25 drive means for selectively moving said vehicle; 

sensor means mounted at a known orientation and position on said 
vehicle, said sensor means providing 3-D imaging data representative of at 
least a selected volume about said vehicle; 

processing means for receiving said sensor data, processing said sensor 
30 data in accordance with a pre-determined instruction set so as to locate one or 
more fixed points, determining the position of said vehicle with respect to said 
fixed points in the selected volume, and so determine the position and 
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orientation of said vehicle and generate control signals for said drive means. 

The invention further includes apparatus enabled to implement the 
invention, and a system for controlling a set of autonomous vehicles using the 
inventive method. 

5 The invention relates to a method for using knowledge of the position of 

the sensor, in three dimensions, relative to a known reference or fixed features 
to track the position of the sensor in three dimensions over a period of time. The 
sensor produces data in three dimensions which are measurements of the 
distance and bearing to objects in the field of regard of the sensor; that is the 

10 sensor is a three dimensional imaging system'. The data is used to determine 
the position of the sensor relative to fixed features in the surroundings of the 
sensor. Knowledge of the position of the sensor relative to a fixed object or 
objects in three dimensions completely determines the local position of the 
sensor. To measure movement, the position of the sensor relative to a fixed 

1 5 object or set of objects must be known in three dimensions at one time and must 
then be determined at a second time. The movement of the object is then 
determined directly from the difference between the two positions. Motion of the 
sensor may be tracked by successive determinations of the change in position 
of the sensor. The sensor may be mounted on equipment such as a vehicle or 

20 other machinery and used to track the movement of the equipment. 

The movement of the sensor can be determined from changes in position 
relative to natural features or characterising features intended to provide specific 
reference points for guidance. The three dimensional spatial relationship of the 
sensor and features in the sensor environment is the key data used to track the 

25 sensor - not a predetermined map or an external reference system. Changes in 
position may be determined relative to a fixed point or a set of fixed points. The 
total movement of the sensor over a period of time can be determined by 
determining the position of the sensor at a succession of times. When starting 
from a known or predetermined position, the true position of the sensor at a 

30 specific time can be determined from knowledge of the starting position and 
knowledge of the movement of the sensor from the starting position up to that 
time. 
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A fixed object or a succession of fixed objects is used to determine the 
position of the moving sensor at successive times and the absolute position of 
the moving sensor relative to the known position from which it started may be 
determined at each of these times and the moving sensor thereby tracked. An 
5 object, for instance a vehicle, fitted with a three dimensional sensor system and 
a processing system to execute the algorithm can therefore track the position in 
three dimensions of the object carrying the sensor using knowledge of the 
starting position and knowledge of the movement in three dimensions. When 
fitted with a suitable three dimensional sensor and processing system the object 
1 0 can track its motion and determine its position relative to its starting point without 
requiring transmission of information to the object such as transmissions of radio 
waves as used in the Global Positioning System (GPS) or sensing elements of 
the motion of the object such as velocity or acceleration as used in inertial 
guidance systems. The three dimensional information acquired by the sensor 
1 5 consists of spatial measurements and embodies no scaling of the data. The 
representation of features therefore provides information as to the position and 
structure of objects in the field of view directly. 

The invention described is a means of using knowledge of the three 
dimensional position relative to the surroundings to track the motion and thus 
20 navigate from a known starting position or relative to sensed features known to 
be fixed. The knowledge of the relative three dimensional position is obtained 
using a three dimensional imaging system. 

The object may be, for example, an autonomous vehicle, wherein the 
location method described is used for navigation. The reference location may 
25 be predefined elements in whole or part where the environment is well defined. 
However, they may equally be determined by an instruction set (software) 
provided on the vehicle, according to some criteria to ensure the feature will be 
locatable after subsequent movement. 

The sensing arrangement may be any suitable sensor which provides a 
30 direct indication of the absolute position or displacement of points relative to the 
sensor. As the arrangement of the sensor on the object is known, the sensing 
arrangement can also provide data about the orientation or attitude of the object, 
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as the orientation of the reference feature will change with changes in the object 
orientation. 

It will be appreciated that a key advantage of the inventive arrangement is 
that it does not require the environment to be fully defined or fitted with carefully 
5 plotted reference points. The inventive method simply selects suitable fixed 
features or parts of fixed features as reference points in transit. It does not rely 
on referencing to the predetermined location of the points, and so elaborate set 
up arrangements as are necessary in the prior art are not required. 

It will be appreciated that the present invention includes a method of 

10 navigation, wherein location is sensed as above, and appropriate direction, 
acceleration and velocity decisions are made in accordance with software 
instructions. Many such proposals are described in the prior art - it is the 
underlying location scheme which is of key significance and difference to the 
prior art. In a navigation system, the vehicle will be provided with an intended 

1 5 destination or waypoint defined in three dimensions, conveniently relative to the 
start position or some agreed reference point. 

The inventive arrangement may be used in conjunction with an INS or 
other positioning system to refine the position estimates and increase the 
accuracy or reliability of either means of estimating position. The inventive 

20 arrangement may be combined with or work with a collision avoidance or 
obstacle avoidance system to provide a plurality of functions. 

Considering an environment such as a mine, the advantages of the 
present invention will become apparent. In a mine, the floor over which a 
vehicle travels is not uniform in smoothness, grade or surface composition. Any 

25 techniques that rely on assumptions about the operating surface being a plane 
will not be operative. Sensing based upon steer angle or wheel rotations will be 
inaccurate, and pot holes and wheel spins will alter the apparent distance 
travelled. Also, in a mine environment, the location in depth may be as 
important as the two dimensional location, and hence techniques reliant on 

30 range estimation based upon height of a target over a plane will be ineffective. 

In an environment such as a mine, the shape of the environment alters on 
a regular basis, due to the nature of extraction of material from the environment. 
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It is also an environment where extraneous features may be added, due to 
spillage, new working, etc. Hence, as the inventive arrangement does not 
require elaborate advance mapping, it is ideally suited to such an environment. 
Whilst the prior art teaches to use a sequence of monocular (camera) 

5 images for the determination of motion, it does not teach the use of matching of 
features (which may be natural or artificial) in multiple three dimensional images 
in all three dimensions to determine the position and orientation of an object in 
the three dimensional space of the field of view of the sensor, which thus allows 
determining changes in the position of the sensor in three dimensions and 

1 0 tracking the actual position of the sensor. 

Known means of determining position include devices such as 
ultrasound sensors or other range measurement devices or two dimensional 
imaging systems such as video cameras. These devices provide information in 
one or two dimensions directly. The acquisition of information in three 

15 dimensions requires data from more than one sensor and, in some cases, 
extensive computation of the data from a number of sensors. A three 
dimensional imaging system may be comprised of a combination of such 
sensors and processors or may be a special purpose three dimensional sensor 
such as a three dimensional laser range measurement scanning system. For 

20 example, two dimensional imaging systems only provide information on the 
angular relationships between features directly and provide no scale but may 
provide three dimensional information indirectly. To obtain three dimensional 
information using a two dimensional imaging system generally requires a great 
deal of computation. Position determination is based on the measurement of 

25 position relative to a known reference such as terrestrial landmarks or stars or 
on the determination of movement from a known position as in an inertial 
navigation system. Position consists of three components. These may be x,y,z 
coordinates in a Cartesian reference frame or on the surface of the earth these 
may be latitude, longitude and elevation relative to the geoid. In many 

30 applications only one component of the position relative to the known reference 
can be determined. This component may be the bearing to the reference 
position as used in navigation from maps using triangulation or the distance to a 
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reference as used in the Global Positioning System where the reference 
satellrte in a known orbit. When only one component of relative position to 
specified reference is known, complete determination of the position of an 
object requires the knowledge of this component for a number of reference 
5 positions. For example, when navigating by map and compass, two bearings 
are used to determine the position of the object. Three bearings are used to 
improve the accuracy of determination of the position of the object. When 
navigating by map and compass the combination of compass bearing and map 
provide the estimate of position in three dimensions. When all three 

10 components of position relative to a reference position are known, the position 
of an object is fully determined within some limits imposed by measurement 
error. A three dimensional imaging system provides knowledge of all three 
position components of objects in the field of view of the sensor and therefore 
fully determines the position relative to the sensor of an object in the field of view 

15 of the sensor. 

Brief Description of the Drawings 

One embodiment of the present invention will now be described with 
reference to the accompanying drawings, in which: 

Figure 1 is a representation of a vehicle and sensor in accordance with 

20 one aspect of the present invention. 

Figure 2A j s a representation of the sensor as it may be installed on a 

vehicle or other suitable object; 
Figure 2B describes the field of view of typical sensors in three 

dimensions; 

25 Figure 3 illustrates how a suitable sensor system may determine its 

position at some time relative to a fixed object. 
Figure 4 illustrates how the sensor may determine its position relative to 

the fixed object at a later time and how the movement of the 
sensor and thus the object carrying the sensor is determined 
30 from the measurement of position at the two separate times. 

Figure 5 illustrates operation of the sensor in two dimensions to 

determine 2-dimensional position in a plane. 
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Figures 5A.5B illustrate a flow chart of the steps involved in the method of the 

present invention. 
Detailed Description of the Preferred Embodiment 

Whilst the operation of the invention is described with reference to a 
5 particular implementation, it will be appreciated that many alternative 
implementations are possible. 

The operation of an illustrative tracking system is as follows. Typically, 
the sensor system includes a motorised vehicle (object) 20 as shown in figure 1 , 
and a sensor 10 mounted atop the vehicle. In one embodiment, vehicle 20 has 

10a main body 22 and tracks 23. In practice however, vehicle 20 may have any 
other configuration and means of locomotion. The sensor 10 will provide an 
image in three dimensions of any features 30 in the proximity of the vehicle. 
Determination of the position of the object 20 carrying the sensor 10 relative to 
any point on a fixed object 20 in the image is then readily performed. To 

1 5 determine motion of the object carrying the sensor it is not necessary to know 
the true position of any feature 30 in proximity to the object. It suffices that the 
features 30 recognised are fixed or do not move during the time the object 20 is 
moving (see figures 3 and 4). In a sequence of two or more images, the 
positions in three dimensions of the object 20 carrying the sensor 10 relative to 

20 features 30 in the proximity of the object are determined for each image in the 
sequence. These positions thus provide measurement of the motion of the 
object carrying the sensor relative to the fixed objects in the proximity. As the 
object 20 carrying the sensor 10 moves, new fixed objects will enter the 
proximity of the sensor and can be used to provide an ongoing measurement of 

25 the motion of the object carrying the sensor. Figure 2B provides a 
representation of object 20 and sensor 10, with the "field of view" of sensor 10 
defined by volume 56. A data processing system 21 such as a computer is used 
to match like features in the three dimensional images. The identification of the 
position of a like feature relative to the sensor 10 is used to measure the 

30 displacement of the feature relative to the sensor. In use, three dimensional 
'images' of a volume 40 containing the feature 30 are obtained at two separate 
times producing feature images 31 and 32. The images are compared and the 
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displacement of the feature in the three dimensional image determined by 
identifying the displacement or offset 33 within the images at which the feature is 
determined to be positioned. In this manner the knowledge of three 
dimensional structure and the position of a sensor relative to the three 
5 dimensional structure is used to determine motion relative to a random set of 
fixed objects. 

To determine the position relative to a feature in the three dimensional 
image, the feature 30 must be identified in the image. If the feature has an axis 
of symmetry, the determination of the change of position and orientation of the 

10 sensor relative to the feature will not be complete. To determine fully the 
orientation and position of the sensor relative to features which have symmetry, 
a number of features are required, for example, the orientation and position of 
the sensor relative to cylindrical poles requires that the sensor 10 see two poles 
to fully determine its position and orientation. In a sequence of two or more 

15 images the same feature or features must be recognised, (i.e. detected and 
identified) in each image and their positions and orientations relative to the 
sensor must be determined. A computer or digital signal processor 21 is used to 
process the data obtained by the sensor 10 and identify the features used to 
determine the positions of the object 20 in a sequence of images. A number of 

20 methods can be used to identify the features in images and match the features 
in different images. These methods include area based, edge based, feature 
based, model based and multilevel or hierarchical approaches and may be 
based on matching methods such as minimisation of least squares measures of 
disparity or determination of the degree of correlation between two features 

25 using devices intended to determine the degree of correlation (correlators). 
These methods may operate on spatial data alone or may operate on a 
combination of spatial and optical data as acquired by some types of 3-D 
imaging sensor. In the case of artificial features whose position is known, the 
true position of the vehicle 20 carrying the sensor 10 is fully determined from the 

30 three dimensional image data at any time that the sensor 10 can "see" the 
artificial feature. In the preferred embodiment, the process of tracking the 
moving object is shown in figures 6A and 6B. At stage 400, the system is 
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initiated by determining the position of object 20 in three dimensions, when the 
object is at a known position. This may be done for example by simply manually 
loading the known coordinates of the object's location into the processor. In 
step 410, the system obtains data, via sensor 10, indicative of the three- 

5 dimensional location of one or more fixed features 30. The processor 21 of 
object 20 then processes the obtained data to determine the three-dimensional 
position of object 20 relative to the fixed feature or features 30. In step 420, 
object 20 moves to a new location. Step 430 involves obtaining new data 
indicative of the new three-dimensional location(s) of the known fixed feature(s) 

10 30 relative to object 20. Using this data in step 440, the processor 21 then 
determines the displacement of object 20 in three-dimensions, from the 
previously known location. The actual three-dimensional coordinates for the 
new location of the object can then be determined in step 450 from the 
knowledge of the coordinates of the previous location and the displacement 

1 5 calculated above. 

In step 460, processor 21 checks to see if certain predetermined 
conditions have been satisfied. For example, a predetermined condition may be 
a destination location. If the conditions are satisfied (e.g. object 20 has reached 
its destination), processor 21 generates control signals causing object 20 to 
20 stop. If the conditions are not satisfied, the process continues to element 402 
which proceeds to step 470 in figure 6B. 

At step 470, sensor 10 obtains additional data indicative of the new three- 
dimensional locations of the known fixed reference features. Note that this step 
need not be undertaken if the data obtained in step 430 is still suitable for use. 
25 For example, if the feature or features used in step 430 may not be visible from 
the new location of object 20 once it has moved again, then step 470 is 
undertaken to select new and more appropriate reference points. The reference 
point chosen need not be a new structure. It may be a more visible feature of 
-the-existing structure, or an extension of the existing structure. 
30 In step 480, processor 21 may also elect to obtain data of new features or 

structures which have come into view since object 20 moved. If this step is 
selected, then appropriate data indicative of the three-dimensional location of 
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the new feature relative to object 20 is obtained via sensor 10, at step 490. 

If step 480 results in a negative, i.e. there are no new features visible, 
then processor 21 skips ahead to junction element 403, omitting step 490. 

From element 403, processor 21 proceeds to element 401 to loop back to 
5 step 420 in figure 6A to move object 20 and repeat the sequence. This loop is 
repeated until the predetermined conditions are satisfied in step 460, and object 
20 stops. 

Sensor 10 and processor 21 may be used to determine the relationship 
between objects and estimate the position of the object carrying the sensor in a 

10 number of ways based on this simple process. For example multiple objects 
may be used. Objects in the sensor field of view may be compared to determine 
the relationship of these objects relative to each other and thus determine 
whether they are moving relative to each other and eliminate moving objects 
from the estimation of position. 

15 A simpler implementation may be realised by the use of a spatial position 

and orientation sensor 60 which acquires information describing the actual 
spatial position relative to the sensor of objects 70, 71, 72 in the field of view of 
the sensor in two dimensions (see figure 5). In this implementation, sensor 60 
acquires spatial data in a plane 80 and the position of the sensor relative to 

20 features 70, 71, 72 in the environment is determined within the plane of 
measurement. Movement relative to features in the environment is then 
characterised by changes in position within the plane of measurement, for 
example the surface of the sea or ocean in the case of a surface vessel. The 
method for estimating motion in two dimensions is analogous to the method 

25 used to estimate motion in three dimensions described previously but eliminates 
the use of the spatial position component which is not contained by the plane of 
measurement. The sensor 60 may be a laser range measurement system such 
as a laser rangefinder or a radar system which is rotated (61) and used 
repetitively so as to measure the range(s) to features in the plane of rotation. 

30 Knowledge of the direction in which the rangefinder is oriented at the time at 
which a range is measured then completely determines the position of a feature 
in the plane of measurement. Figure 4 illustrates the operation of the invention 
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in two dimensions. 

A key aspect of the present invention is the use of the characteristics of a 
true 3-D image to navigate. Two dimensional data such as position in a plane 
determined by a laser scanner, angular position such as determined by an 
5 imaging system, one dimensional data such as range or bearing and single 
point three dimensional data such as range combined with azimuth and 
elevation and bearing to a single point can not be used to navigate in three 
dimensions. 

When three dimensional data is acquired from a sensor the position of 
10 the sensor in the object space is completely determined and this thus 
differentiates a three dimensional sensor system from one and two dimensional 
sensors which must be used in conjunction with additional knowledge such as 
knowledge of a map or other sensors to determine the position of the sensor. 
With a three dimensional system, changes in the position of the sensor can be 
1 5 readily tracked in three dimensions and knowledge of the true position of the 
sensor maintained. If the sensor is mounted on an object such as a vehicle and 
the position of the sensor on the vehicle known and the orientation of the sensor 
relative to the vehicle known then the position of the vehicle is known and can 
be tracked. The tracking can be performed in three dimensions without the use 
20 of artificial features or the requirement to know the position of selected artificial 
or natural features. 

The present implementation is discussed primarily with reference to the 
use of laser based range measurement implementations. However, numerous 
alternative 3-D imaging systems may be used to implement the inventive 
25 concept, some of which are discussed briefly below. 
Alternative methods of producing 3-D images 

Two primary methods are employed for 3-D imaging: 
measurement of range and orientation to a plurality of points including 
scanning range measurement devices, and 
30 triangulation to a plurality of points. 

In addition 3-D images may be produced by: 
Range gated imaging 
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Moire techniques 

Holographic interferometry 

Adaptive focussing systems 

Estimation of range from image defocus 
5 Each of these will be discussed below. 

Scanning range measurement devices 

Three dimensional images are acquired using scanning range 
measurement devices by recording the range to points in the object space at 
which the range measurement device is directed. When the range 
10 measurement device is scanned over the object space and the range recorded 
over a grid of points for which the position and orientation of the sensor is 
known, a range map is produced which is then readily converted to a three 
dimensional image. The use of a laser range finder scanned over an object 
space to produce a three dimensional image has been demonstrated many 
1 5 times. Ultrasonic or radar range finders can be used to generate similar images. 
Triangulation systems 

Triangulation systems require that we have two components of the sensor 
which each allow us to determine the direction in three dimensions to a point in 
the object space. The position and orientation of each component of the sensor 
20 is known and, since for each component of the sensor the angular direction to 
the point is known, the position in space of the point is readily determined thus 
providing a three dimensional image. Triangulation may be subdivided into 
techniques: passive triangulation and active triangulation. 

Passive triangulation encompasses such techniques as aerial or 
25 terrestrial photogrammetry where the components of the measurement system 
are two or more cameras, or one camera taking two or more images. Points in 
each image are matched and from the position in each image of the matched 
points the spatial position is determined. Fast systems using two television 
cameras and image processing systems are sometimes classified as stereo 
30 vision. 

Active triangulation encompassed such techniques as structured light 
imaging where a light stripe is projected into an object space and viewed by a 
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camera or similar sensor from another position. In some instances two cameras 
are used. Knowledge of the direction of projection of the light stripe and the 
position and orientation of the camera or cameras enables calculation of the 
position in space of any point reflecting the light. Another form of active 
5 triangulation involves the use of a spot of light scanned over the object space. 
Range Gated imaging 

Range gated imaging is achieved by obtaining a series of images of a 
scene which is illuminated with a controlled light source. The light source has 
an extremely short duration and is turned on and off. Each time that the light 

1 0 source is turned on an image of the scene is obtained for an extremely short 
time after a variable delay. Those features in the scene which reflect light during 
this period and are therefore at a known range from the camera thus produce an 
image. By acquiring a series of images of this form a range map of the scene is 
obtained. The position and orientation of the sensor is known and, since for 

1 5 each image the angular direction of the features is known, the range map is then 
converted to a three dimensional image. 
Moire Techniques 

A Moire pattern is an interference pattern of low spatial frequency. The 
interference pattern is projected into the object space and surface depth 

20 information is encoded in and recovered from the phase information embodied 
in the observed interference pattern in the object space. 
Holography 

Holographic interferometers use coherent radiation to produce 
interference patterns. When optical radiation is used, a photodetector is used to 
25 detect the reflected radiation and the three dimensional image is constructed 
from the structure of the interference pattern. Interferometry may be performed 
with radiation of wavelengths typically used for radar systems. 
Adaptive Focussing 

The optical configuration required to achieve focus of an imaging system 
30 can be used to infer the range to a feature in the object space. By varying the 
focus and recording the focus conditions, e.g. the optical configuration required 
to achieve focus, when a feature is in focus the range to features in the object 
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space is mapped. This data is treated similarly to the range and image data in a 
range gated imaging system to produce a three dimensional image. 
Estimation of range from image defocus 

The degree of defocus of an imaging system can be used to infer the 

5 range to a feature in the object space. The estimation of the range to an object 
is achieved by modelling the effect that the optical configuration of a camera, 
namely the range at which best focus is achieved, has on images acquired with 
a small depth of field. Range information is recovered from defocused images 
by calculating the range of various points in an image by estimating the degree 

0 of defocus at the said points. These methods are discussed, for example in, 
Rajagopalan A.N., Chaudhuri Space Variant Approaches To Recovery of Depth 
From Defocused Images; Computer Vision and Image Understanding Vol 68, 
No 2, December 1997. 

Obviously other alternative 3-D imaging techniques can be employed to 

5 implement the present invention. For example, as discussed in Besl, P.J., Active 
Optical Range Imaging Sensors; Machine Vision and Applications Vol 1 1988. 

It will be appreciated that the invention has been described in terms of a 
preferred embodiment. Many variations and modifications are possible within 
the scope of the present invention as defined by the following claims. 
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CLAIMS: 

1 . A method for determining the position of a moveable object, including the 
steps of: 

a) initiating the method when said object is at an initial known 
location in three-dimensions; 

b) obtaining data indicative of the three-dimensional location relative 
to said object of one or more fixed reference features by way of one or more 
sensors associated with said object to determine the displacement of said object 
from said reference feature(s); 

c) moving said object to a new object location; 

d) at the new object location, obtaining new data indicative of the new 
location of said fixed reference feature or features relative to said object via said 
sensor(s); 

e) determining from said new data, the displacement of said object 
relative to said fixed feature or features; 

f) determining from said displacement of said objection and 
knowledge of said initial known location, the new three-dimensional location of 
said object; 

g) if the object is moving out of view of said fixed reference feature(s), 
and/or when a new fixed reference feature or features come into view, obtaining 
data via said sensor(s) indicative of the three-dimensional location of said new 
fixed feature or features which may be used as new reference locations relative 
to said object; and 

h) repeating steps (c) to (g) as required until predetermined 
conditions are fulfilled. 

2. A method according to claim 1 including, between the steps of (f) and (g), 
obtaining additional data indicative of the three-dimensional location of the fixed 
feature(s), or of extensions of said fixed feature(s) if the new data obtained in 
step (d) is no longer suitable for use as reference data. 
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3. A method according to any one of claims 1 or 2 wherein said data 
indicative of three-dimensional location of said one or more reference features 
includes optical reflectance data. 

4. A method according to claim 3 wherein said fixed feature or features form 
part of a natural feature and/or an artificial feature defining a three-dimensional 
object. 

5. A vehicle including; 

drive means for propelling the vehicle along a path; 

one or more sensors mounted at a known orientation and position on 
said vehicle, said one or more sensors providing three-dimensional data 
representative of at least a selected volume about said vehicle; 

processing means for receiving said data from said sensor and 
processing said data to determine at least one of: 

the location of one or more fixed reference points; 

the position of said vehicle relative to said one or more fixed reference 
points; and 

the location of said vehicle having knowledge of a previously known or 
determined location in three-dimensions. 

6. A vehicle according to claim 5 wherein said processing means also 
generates control signals for said drive means. 
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